/* 
 * University of Plymouth Bioloid Robot Controller
 * for Atmel Mega 128
 * FIRA England Robot Football Team
 * - Based on Dynamixel Evaluation Example (Original Author : Byoung Soo Kim)
 * - Innitial support for executing Motion Editor Pages
 * Author : Joerg Wolf, joerg.wolf -xaxtx- plymouth.ac.uk
 * 
 * Version 0.11
 */
#define ENABLE_BIT_DEFINITIONS 
#include <avr/io.h>
#include <avr/pgmspace.h>
#include <avr/interrupt.h>
#include <avr/signal.h>
#include <inttypes.h>
#include <string.h>
#include <stdio.h>
#include "motion.h"
#include "IMU.h"
#include "AX-12.h"

volatile MOTION_HEARTBEAT_DATA MHD;
volatile int ServoPos[SERVO_MEMORY];
volatile int ServoTrim[SERVO_MEMORY];

volatile uint8_t Balance;

// ----------- extern functions from led.c ---------------
extern void LED_Init(void);
extern void Set_LED(unsigned char LEDno, unsigned char state);

// ----------- extern functions from comms.c ---------------
extern void TxDString(byte *bData);
extern byte RxD81(void);

extern byte gbpParameter[128];
extern byte gbpRxBuffer[128]; 
extern byte gbpTxBuffer[128]; 
extern byte TxPacket(byte bID, byte bInstruction, byte bParameterLength);
extern byte RxPacket(byte bRxLength);
extern void PrintBuffer(byte *bpPrintBuffer, byte bLength);
extern volatile unsigned int ROBOTSTATUS;



uint16_t doPose(int page,char pose);
uint16_t doPage(int page);
void ContinueMotion_HeartBeat(void);
void MotionInitialise(void);


uint16_t abs(int16_t n)
{
	if(n<0){ return -n;}
	return n;
}

int getCurrentServoPos(uint8_t ServoNo)
{
	uint16_t POS_LB=0,POS_HB=0;
	gbpParameter[0] = P_PRESENT_POSITION_L;
	gbpParameter[1] = 2; //Read Length 2 byte L and H
	TxPacket(ServoNo,INST_READ,2);
	uint8_t bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1]);
	if(bRxPacketLength == DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1]){
		if(gbpRxBuffer[4]==0){
			POS_LB=gbpRxBuffer[5];
			POS_HB=gbpRxBuffer[6];
		}else{
			// error
			return SERVO_NOT_CONNECTED;
		}
	}else{
		//error
		return SERVO_NOT_CONNECTED;
	}
	return (int)((POS_HB<<8) + POS_LB);
}


void MotionInitialise(void){
	memset((void *)&MHD, 0, sizeof(MOTION_HEARTBEAT_DATA));
	MHD.CurrentPage = -1;
	
	uint8_t ServoNo;
	
	for( ServoNo=1;ServoNo <= NUM_OF_SERVOS_ATTACHED; ServoNo++){
			ServoPos[ServoNo] = 0;
			ServoTrim[ServoNo] = 0;
			ServoPos[ServoNo] =  getCurrentServoPos(ServoNo);
			if(ServoPos[ServoNo] == SERVO_NOT_CONNECTED){
				char temp[30]; sprintf(temp,"E:Servo %i not connected\n\r",ServoNo); TxDString(temp);
			}
			else{
				AX12WriteUShort(ServoNo,AX12_TORQUE_ENABLE,1);  //Set Torque Enable off
				char temp[30]; sprintf(temp,"Servo %i connected\n\r",ServoNo); TxDString(temp);
				}
			//char temp[50]; TxDString("ServoPos:");sprintf(temp," %i",ServoPos[ServoNo]); TxDString(temp);
			/*
			// Get Lower Byte of Current Position
			uint16_t POS_LB=0,POS_HB=0;
			gbpParameter[0] = P_PRESENT_POSITION_L;
			gbpParameter[1] = 1; //Read Length
			TxPacket(ServoNo,INST_READ,2);
			uint8_t bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1]);
			if(bRxPacketLength == DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1]){
				if(gbpRxBuffer[4]==0){
					POS_LB=gbpRxBuffer[5];
				}else{
					// error
					ServoPos[ServoNo] = SERVO_NOT_CONNECTED;
				}
			}else{
				//error
				ServoPos[ServoNo] = SERVO_NOT_CONNECTED;
			}
			
			// Get Higher Byte of Current Position
			gbpParameter[0] = P_PRESENT_POSITION_H;
			gbpParameter[1] = 1; //Read Length
			TxPacket(ServoNo,INST_READ,2);
			bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1]);
			if(bRxPacketLength == DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1]){
				if(gbpRxBuffer[4]==0){
					POS_HB=gbpRxBuffer[5];
				}else{
					// error
					ServoPos[ServoNo] = SERVO_NOT_CONNECTED;
				}
			}else{
				//error
				ServoPos[ServoNo] = SERVO_NOT_CONNECTED;
			}
			
			// CODE 9999 means the servo is not attached
			if(ServoPos[ServoNo] != SERVO_NOT_CONNECTED){
				ServoPos[ServoNo] = (POS_HB<<8) + POS_LB;
				//char temp[50]; sprintf(temp," %i",ServoPos[ServoNo]); TxDString(temp);
			}else{
				char temp[50]; sprintf(temp,"E:Servo %i not connected\n\r",ServoNo); TxDString(temp);
			}
			*/
	}
}

/*
//!\\ SERVO KILLER FUNCTION
void askInfo(void) {
	unsigned char length = AX12_PRESENT_TEMPERATURE - AX12_GOAL_POSITION_L + 1;
	unsigned char cData[length];
	unsigned char i;

	for(i=7 ; i <= 18 ; i++)
		AX12ReadMemory (i, AX12_GOAL_POSITION_L, length, cData);
}
*/

void ContinueMotion_HeartBeat(void)
{
	uint8_t MOVING = FALSE;

//!\\ SERVO KILLER FUNCTION
// 	askInfo ();
	
// 	BalanceControl( MHD.CurrentPage, MHD.CurrentPose);
	
	
	if(MHD.CurrentPage != -1){
		
		// Check if Servos are still in Motion
		/*
		gbpParameter[0] = P_MOVING;
		gbpParameter[1] = 1; //Read Length
		for( ServoNo=1;ServoNo <= NUM_OF_SERVOS_ATTACHED; ServoNo++){	
			TxPacket(ServoNo,INST_READ,2);
			uint8_t bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1]);
			if(bRxPacketLength == DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1]){
				if(gbpRxBuffer[4]==0){
					//TxDString("M:");TxD8Hex(gbpRxBuffer[5]);
					if(gbpRxBuffer[5]!=0){
						// Servo is still moving
						MOVING = TRUE;
					}
				}else{
					TxDString("Error:");//gbpRxBuffer[4]
				}
			}
		}
		*/
		
		
		if(MHD.MoveFinishTime>0){MHD.MoveFinishTime--;}
		
		if(MHD.MoveFinishTime==0){
			MOVING = FALSE;
			//TxDString("M");
		}else{
			MOVING = TRUE;
		}
		
		if(MOVING == FALSE){ 
			if(MHD.PosePause>0){MHD.PosePause--;}
		
		if(MHD.PosePause==0){
				//TxDString("+\n\r");
				if(ROBOTSTATUS!=STOP_POSE){
					if(MHD.CurrentPose+1<MHD.NoOfPoses){
						// next pose
						MHD.CurrentPose++;
						//TxDString("---Next Pose---\n\r");
						MHD.PosePause = doPose(MHD.CurrentPage,MHD.CurrentPose);
					}else{
						// finished
						MHD.PlayCount--;
						if(MHD.PlayCount > 0) {
							MHD.CurrentPose = 0;
							MHD.PosePause = doPose(MHD.CurrentPage,MHD.CurrentPose);
						}else{
							//TxDString("Done Page\n\r");
							char txt[11]; txt[0]='\0';
							sprintf(txt,"[pg%02X]",MHD.CurrentPage);
							TxDString(txt);
							if(MHD.NextPage != 0){
								if(ROBOTSTATUS!=STOP_PAGE){
									//TxDString("NEXT PAGE\n\r");//NextPage;
									doPage(MHD.NextPage);
								}
							}else{
								MHD.CurrentPage=-1;
extern int UsingPoseSystem;
UsingPoseSystem = 0;
							}
						}
					}
				}
			}
		}
	}
}

void stopLoopWithNextPage (int nextPage) {
	if(MHD.CurrentPage != -1)
		MHD.NextPage = nextPage;
}

uint16_t doPage(int page)
{
	//char temp[50];
	//sprintf(temp,"Start doPage(%i)\n\r",page);
	//TxDString(temp);
	
extern int UsingPoseSystem;
UsingPoseSystem = 1;

	if(page>127)page=127;//(MHD.CurrentPage != page)
	if(page!=-1){
		// New request
		MHD.CurrentPage = page;
		cli();
		MHD.NoOfPoses= pgm_read_byte_far ((uint32_t)POSE_BASE_ADR +(uint32_t)POSE_PAGE_SIZE*(uint32_t)MHD.CurrentPage + (uint32_t)POSE_PAGE_NUM_OF_MOTIONS);
		MHD.PlayCount = pgm_read_byte_far ((uint32_t)POSE_BASE_ADR +(uint32_t)POSE_PAGE_SIZE*(uint32_t)MHD.CurrentPage + (uint32_t)POSE_PAGE_PLAYCOUNT);
		MHD.NextPage = pgm_read_byte_far  ((uint32_t)POSE_BASE_ADR + (uint32_t)POSE_PAGE_SIZE*(uint32_t)MHD.CurrentPage + (uint32_t)POSE_PAGE_NEXT_PAGE);
		sei();
		MHD.TotalTime=0;
		MHD.CurrentPose = 0;
		MHD.PosePause = doPose(MHD.CurrentPage,MHD.CurrentPose);
		//sprintf(temp,"First PosePause=%i\n\r",MHD.PosePause);TxDString(temp);
		return MHD.TotalTime;
	}
	return 0;
}


void SendServoTargetPos(uint8_t ServoNo, int16_t ThetaTarget,uint16_t OmegaServo)
{
	switch (ServoNo)
	{
	case 3:
		if (ThetaTarget < 250) ThetaTarget = 250;
		break;
		
	case 4:
		if (ThetaTarget > 774) ThetaTarget = 774;
		break;
	}	

	//AX12SetPosition((unsigned char)ServoNo, (unsigned short)ThetaTarget, (unsigned short)OmegaServo);

	uint8_t servo_value_LB = ThetaTarget & 0x00FF;
	uint8_t servo_value_HB = (ThetaTarget & 0xFF00)>>8;
	
	// send servo value
	gbpParameter[0] = P_GOAL_POSITION_L; 
	gbpParameter[1] = servo_value_LB; //Writing Data P_GOAL_POSITION_L
	gbpParameter[2] = servo_value_HB; //Writing Data P_GOAL_POSITION_H
	gbpParameter[3] = (OmegaServo & 0x00FF); //Writing Data P_GOAL_SPEED_L
	gbpParameter[4] = (OmegaServo & 0xFF00)>>8; //Writing Data P_GOAL_SPEED_H
	
	uint8_t TRANSM_OK = FALSE;
	uint8_t ATTEMPT=0;
	//do{
		TxPacket(ServoNo,INST_WRITE,5);
		uint8_t bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE);
		ATTEMPT++;
		if(ATTEMPT>10){TRANSM_OK=TRUE;}// give up
		
		if(bRxPacketLength != DEFAULT_RETURN_PACKET_SIZE){
			//TxDString("E1");
			char temp[30];
			sprintf(temp,"E:E1[%i]\n\r",ServoNo);TxDString(temp); 

			// try to receive more bytes until finished to regain sync
			//RxPacket(DEFAULT_RETURN_PACKET_SIZE);
			// We should try to send the command again here
		}else if(gbpRxBuffer[4]!=0){
				char temp[30]; sprintf(temp,"E:E2[%i]\n\r",ServoNo);TxDString(temp);		
				// We should try to send the command again here
		}else{
			TRANSM_OK=TRUE;
		}
	//}while(TRANSM_OK!=TRUE);
}



uint16_t doPose(int page,char pose)
{
	char temp[100];
	uint8_t PoseSpeed,ServoNo;
	uint16_t PosePause;
	uint32_t PoseAdr;
	
	
	if(MHD.CurrentPage!=-1){
		
		
		cli();
		uint8_t NoOfPosesinPage = pgm_read_byte_far ((uint32_t)POSE_BASE_ADR + (uint32_t)POSE_PAGE_SIZE*(uint32_t)page + (uint32_t)POSE_PAGE_NUM_OF_MOTIONS);
		sei();
		if( pose+1 > NoOfPosesinPage){
			TxDString("E:Pose not on this page\n\r");
			return 0;
		}
		cli();
		PosePause = pgm_read_byte_far  ((uint32_t)POSE_BASE_ADR + (uint32_t)POSE_0_OFFSET + (uint32_t)POSE_PAGE_SIZE*(uint32_t)page + (uint32_t)POSE_SIZE*(uint32_t)pose +(uint32_t)POSE_PAUSE_ADR);
		PoseAdr =  						(uint32_t)POSE_BASE_ADR + (uint32_t)POSE_0_OFFSET + (uint32_t)POSE_PAGE_SIZE*(uint32_t)page + (uint32_t)POSE_SIZE*(uint32_t)pose;
		
		
		PoseSpeed = pgm_read_byte_far  ((uint32_t)POSE_BASE_ADR + (uint32_t)POSE_0_OFFSET + (uint32_t)POSE_PAGE_SIZE*(uint32_t)page + (uint32_t)POSE_SIZE*(uint32_t)pose + (uint32_t)POSE_SPEED_ADR);

		uint16_t PageSpeed = pgm_read_byte_far ((uint32_t)POSE_BASE_ADR +(uint32_t)POSE_PAGE_SIZE*(uint32_t)page + (uint32_t)POSE_PAGE_MOTION_SPEED);
		
		uint16_t PageFast = pgm_read_byte_far ((uint32_t)POSE_BASE_ADR +(uint32_t)POSE_PAGE_SIZE*(uint32_t)page + (uint32_t)POSE_PAGE_FAST_FLAG);
		
		if((PageFast==10)&&(PageSpeed>=10)){PageSpeed=160;}
		sei();
		//uint8_t AccelTime = pgm_read_byte_far  ((uint32_t)POSE_BASE_ADR + (uint32_t)POSE_PAGE_SIZE*(uint32_t)page + (uint32_t)POSE_PAGE_ACCEL_TIME);
		//uint16_t Ta_T0 = (float)AccelTime*k;
		//sprintf(temp,"\r\nPageNo=%i,PageSpeed=%i,PoseSpeed=%i,PosePause=%i\r\n",MHD.CurrentPage,PageSpeed,PoseSpeed,PosePause);TxDString(temp);
		//sprintf(temp,"#%i,%i Sp%i,%i,Pa%i\n\r",page,pose,PageSpeed,PoseSpeed,PosePause);TxDString(temp);
		/**
			Find the Servo that has to move the longest path (ThetaMax)
			This servo will be the only one that moves at the specified speed
			The other servos will move slower so that all servos finish the move at the same time
		*/
		uint16_t ThetaMax = 0;
		for(ServoNo=1;ServoNo <= NUM_OF_SERVOS_ATTACHED; ServoNo++){
			if(ServoPos[ServoNo] != SERVO_NOT_CONNECTED){
				/*
				cli();
				uint8_t servo_value_LB = pgm_read_byte_far  (PoseAdr + (uint32_t)ServoNo*2 );
				uint16_t servo_value_HB = pgm_read_byte_far  (PoseAdr + (uint32_t)ServoNo*2 +1);
				sei();
				
				int16_t ThetaTarget = (servo_value_HB<<8) + servo_value_LB;
				*/
				int16_t ThetaTarget = get_ServoPos_Flash(page,pose,ServoNo);
				ThetaTarget = ThetaTarget + ServoTrim[ServoNo];
				
				if(ThetaTarget < 0) ThetaTarget = 0;
				if(ThetaTarget > 0x3ff ) { 
					ThetaTarget=0x03ff;
					//sprintf(temp,"E:[%i] HB>3\r\n",ServoNo);TxDString(temp);
					//TxDString("[E:HB>3]");
				}
				
				
				
				uint16_t ThetaDiff = fabs((int16_t)ThetaTarget - (int16_t)ServoPos[ServoNo]);
				if(ThetaDiff > ThetaMax){
					ThetaMax = ThetaDiff;
				}
				//sprintf(temp,"\r\ID=%i,Target=%u,Max=%u,D=%u,SP=%u\r\n",ServoNo,ThetaTarget,ThetaMax,ThetaDiff,ServoPos[ServoNo]);TxDString(temp);
			}
		}
		//sprintf(temp,"\r\nThetaMax=%i\r\n",ThetaMax);TxDString(temp);
		
		/**
			Calculate Omega-PageSpeed-PoseSpeed-Normalised:
			Servos move with 90 deg/sec when PageSpeed=32 and PoseSpeed=32
			At this point OmegaPPN is 1.00
		*/
		
		float OmegaPPN = ((float)(PageSpeed*PoseSpeed))/1024.0;
		
		/**
			Calculate how many time steps it will take to finish the move.
			The servo that moves furthest is used with the formulae t = theta/omega
			The conversion factors have to be taken into account:
			- for ThetaMax: one degree in servo position is 3.41 servo units
			- for OmegaPPN: OmegaPPN is 1 at 90 deg/sec
			Using the conversion above gives the time in seconds 
			- in order to change to timesteps divide by 7.8msec
			All in all results in 0.41707
		*/
		MHD.MoveFinishTime = (uint16_t)(0.41707*((float)ThetaMax/OmegaPPN));
		if(MHD.MoveFinishTime > 30*128){TxDString("[E:FinishTime too long?]");}
		//sprintf(temp,"MFT=%i,TM=%i,OPPM=%i\n\r",MHD.MoveFinishTime,ThetaMax,(int)(OmegaPPN*1024.0));TxDString(temp); 
		/**
			Maximum Servo Speed depends on OmegaPPN
			The maximum Dynamixel servo speed command is 0x3ff = 1023 = 114 rpm 
			114 rpm = 1.0 rps = 684 deg/sec
			From an experiment I know that if OmegaPPN =1 the servo speed should be 90 deg/sec
			( 90 / 684 ) * 1023 = 135 servo speed units
			Therefore the conversion factor from OmegaPPN to servo Speed = 135
			
		*/
		uint16_t OmegaServoMax = (OmegaPPN * 135.0);
			
		/* if PageSpeed is set to 32, a Pause in the Pose of 128 will last 1 second */
		PosePause = (PosePause * 32)/PageSpeed;	
		for(ServoNo=1;ServoNo <= NUM_OF_SERVOS_ATTACHED; ServoNo++){
			// little endian order
			if(ServoPos[ServoNo] != SERVO_NOT_CONNECTED){
				/* cli();
				uint8_t servo_value_LB = pgm_read_byte_far  (PoseAdr + (uint32_t)ServoNo*2 );
				uint8_t servo_value_HB = pgm_read_byte_far  (PoseAdr + (uint32_t)ServoNo*2 +1);
				sei();
				
				if(servo_value_HB > 0x03 ) { servo_value_HB=0x03;}
				uint16_t HB = servo_value_HB;
				int16_t ThetaTarget = (HB<<8) + servo_value_LB;
				*/
				int16_t ThetaTarget = get_ServoPos_Flash(page,pose,ServoNo);
				ThetaTarget = ThetaTarget + ServoTrim[ServoNo];
				if(Balance){
					if((ServoNo==11)||(ServoNo==12)||(ServoNo==13)||(ServoNo==14)){
						int error = ServoPos[ServoNo] - getCurrentServoPos(ServoNo);
						ThetaTarget = ThetaTarget + error/2;
						//byte txt[25];
						//if((abs(error)>10)||(abs(error)>10)){
						//	sprintf(txt,"he%i\r\n",error); TxDString(txt);
						//}
					}
				}
				
				if(ThetaTarget > 0x3ff){ThetaTarget = 0x3ff;}
				if(ThetaTarget < 0){ThetaTarget = 0;}
				
				//uint16_t ThetaDiff = abs(ThetaTarget - ServoPos[ServoNo]);  PH 2/3/2009. Replaced by line following
				uint16_t ThetaDiff = fabs((int16_t)ThetaTarget - (int16_t)ServoPos[ServoNo]);
						
				// calculate servo speed
				uint16_t OmegaServo = (uint16_t)((float)OmegaServoMax * ((float)ThetaDiff/(float)ThetaMax));
				if(OmegaServo>0x3ff){OmegaServo=0x3ff;}
				
				SendServoTargetPos(ServoNo,ThetaTarget,OmegaServo);
				
				// remember new target pose
				ServoPos[ServoNo]= ThetaTarget ;
			}
		}
	}else{
			// Page is -1
			
			return 0;
	}
	return PosePause;
}


int16_t get_ServoPos_Flash(int page,char pose,int ServoNo)
{
	uint32_t PoseAdr =  (uint32_t)POSE_BASE_ADR + (uint32_t)POSE_0_OFFSET + (uint32_t)POSE_PAGE_SIZE*(uint32_t)page + (uint32_t)POSE_SIZE*(uint32_t)pose;
	
	cli();	
	uint8_t servo_value_LB = pgm_read_byte_far  (PoseAdr + (uint32_t)ServoNo*2 );
	uint16_t servo_value_HB = pgm_read_byte_far  (PoseAdr + (uint32_t)ServoNo*2 +1);
	sei();
	if(servo_value_HB > 0x03 ) { servo_value_HB=0x03;}
	return (int16_t)((servo_value_HB<<8) + servo_value_LB); 
}


int16_t limit1024( int16_t n){
	if(n < 0) n = 0;
	if(n > 0x3ff ) n=0x03ff;
	return n;
}

IMU_DATA MeanIMU;
double MeanIMUCount = 0;

void MeanIMUReset(void)
{
	MeanIMU.Pitch = 0;
	MeanIMU.Roll  = 0;
	MeanIMUCount  = 0;
}

void MeanIMUAdd(IMU_DATA *IMU)
{
	MeanIMU.Pitch += IMU->Pitch;
	MeanIMU.Roll  += IMU->Roll;
	MeanIMUCount ++;
}

double MeanIMUPitch(void)
{
	return (MeanIMUCount != 0)? (MeanIMU.Pitch / MeanIMUCount): 0;
}

double MeanIMURoll(void)
{
	return (MeanIMUCount != 0)? (MeanIMU.Roll / MeanIMUCount): 0;
}

void BalanceControl(int Page,char Pose)
{
	static int div_tick = -1,
				LastPage = 0,
				LastPose = 0;
				
	IMU_DATA	IMU;
	
	double		ExpectedPitch = 0 * 3.14/180,
				PitchK = -307.0 * 0.05,
				PitchError,
				ExpectedRoll = 0,
				RollK = 307.0 * 0.05,
				RollError;
			
				
	div_tick++;
	
	IMURead(&IMU);
	
	if (div_tick == 0)	//	Then we are initialising
		MeanIMUReset();
	else
		MeanIMUAdd(&IMU);
	
	if ( (Page == 52) || (Page == 45) )
	{
		if (LastPage != Page)						//	Then we have just started the walk
			MeanIMUReset();

		if ( (LastPage != Page) ||					//	Then we have just started the walk or...
			 ( (Pose == 0) && (LastPose != 0) ) )	//	...we have just started a new cycle of an existing walk
		{
			PitchError = ExpectedPitch - MeanIMUPitch();
			RollError  = ExpectedRoll  - MeanIMURoll();
			
			ServoTrim[1]  = (int)(PitchError * -PitchK);	//	Move arms forward/backwards to balance pitch
			ServoTrim[2]  = (int)(PitchError *  PitchK);
			
			ServoTrim[11] = (int)(PitchError *  PitchK);	//	Move hips to balance pitch
			ServoTrim[12] = (int)(PitchError * -PitchK);
			
			ServoTrim[3]  = (int)(RollError *  RollK);		//	Raise arms to balance roll
			ServoTrim[4]  = (int)(RollError *  RollK);
			
			CommsPrintf(",%d,%d,%d,   ", IMURadiansToDegrees(PitchError), IMURadiansToDegrees(RollError), (int)(MeanIMUCount));
			CommsPrintf(",%d,%d,%d\r\n", ServoTrim[2], ServoTrim[11], ServoTrim[3]);
			
			MeanIMUReset();
		}
	}

	LastPage = Page;
	LastPose = Pose;
	
/*	if(div_tick == 10)
	{
		double KPP = 307.0 * 0.8,	//	307 servo units equates to 90 degrees
				ExpectedPitch = -10 * 3.14/180,
				KPR = 307.0 * 0.8;
		
		div_tick=0;
		
		ServoTrim[1]  = (int)((ExpectedPitch - imu.Pitch) * -KPP);	//	Move arms forward/backwards to balance pitch
		ServoTrim[2]  = (int)((ExpectedPitch - imu.Pitch) *  KPP);
		
		ServoTrim[11] = (int)((ExpectedPitch - imu.Pitch) *  KPP);	//	Move hips to balance pitch
		ServoTrim[12] = (int)((ExpectedPitch - imu.Pitch) * -KPP);
		
		ServoTrim[3]  = (int)(imu.Roll * KPR);						//	Raise arms to balance roll
		ServoTrim[4]  = (int)(imu.Roll * KPR);		
	} */
}

